{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Collision Avoidance - Data Collection\n",
    "\n",
    "If you ran through the basic motion notebook, hopefully you're enjoying how easy it can be to make your Jetbot move around! Thats very cool!  But what's even cooler, is making JetBot move around all by itself!  \n",
    "\n",
    "This is a super hard task, that has many different approaches but the whole problem is usually broken down into easier sub-problems.  It could be argued that one of the most\n",
    "important sub-problems to solve, is the problem of preventing the robot from entering dangerous situations!  We're calling this *collision avoidance*. \n",
    "\n",
    "In this set of notebooks, we're going to attempt to solve the problem using deep learning and a single, very versatile, sensor: the camera.  You'll see how with a neural network, camera, and the NVIDIA Jetson Nano, we can teach the robot a very useful behavior!\n",
    "\n",
    "The approach we take to avoiding collisions is to create a virtual \"safety bubble\" around the robot.  Within this safety bubble, the robot is able to spin in a circle without hitting any objects (or other dangerous situations like falling off a ledge).  \n",
    "\n",
    "\n",
    "Of course, the robot is limited by what's in it's field of vision, and we can't prevent objects from being placed behind the robot, etc.  But we can prevent the robot from entering these scenarios itself.\n",
    "\n",
    "The way we'll do this is super simple:  \n",
    "\n",
    "First, we'll manually place the robot in scenarios where it's \"safety bubble\" is violated, and label these scenarios ``blocked``.  We save a snapshot of what the robot sees along with this label.\n",
    "\n",
    "Second, we'll manually place the robot in scenarios where it's safe to move forward a bit, and label these scenarios ``free``.  Likewise, we save a snapshot along with this label.\n",
    "\n",
    "That's all that we'll do in this notebook; data collection.  Once we have lots of images and labels, we'll upload this data to a GPU enabled machine where we'll *train* a neural network to predict whether the robot's safety bubble is being violated based off of the image it sees.  We'll use this to implement a simple collision avoidance behavior in the end :)\n",
    "\n",
    "> IMPORTANT NOTE:  When JetBot spins in place, it actually spins about the center between the two wheels, not the center of the robot chassis itself.  This is an important detail to remember when you're trying to estimate whether the robot's safety bubble is violated or not.  But don't worry, you don't have to be exact. If in doubt it's better to lean on the cautious side (a big safety bubble).  We want to make sure JetBot doesn't enter a scenario that it couldn't get out of by turning in place."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Display live camera feed\n",
    "\n",
    "So let's get started.  First, let's initialize and display our camera like we did in the *teleoperation* notebook.  \n",
    "\n",
    "> Our neural network takes a 224x224 pixel image as input.  We'll set our camera to that size to minimize the filesize of our dataset (we've tested that it works for this task).\n",
    "> In some scenarios it may be better to collect data in a larger image size and downscale to the desired size later."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "from jetbot import Camera, bgr8_to_jpeg\n",
    "#\n",
    "camera = Camera.instance(width=224, height=224)\n",
    "image = widgets.Image(format='jpeg', width=224, height=224)  # this width and height doesn't necessarily have to match the camera\n",
    "camera_link = traitlets.dlink((camera, 'value'), (image, 'value'), transform=bgr8_to_jpeg)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Awesome, next let's create a few directories where we'll store all our data.  We'll create a folder ``dataset`` that will contain two sub-folders ``free`` and ``blocked``, \n",
    "where we'll place the images for each scenario."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Directories are not created because they already exist\n"
     ]
    }
   ],
   "source": [
    "import os\n",
    "\n",
    "blocked_dir = 'dataset/blocked'\n",
    "free_dir = 'dataset/free'\n",
    "\n",
    "# we have this \"try/except\" statement because these next functions can throw an error if the directories exist already\n",
    "try:\n",
    "    os.makedirs(free_dir)\n",
    "    os.makedirs(blocked_dir)\n",
    "except FileExistsError:\n",
    "    print('Directories are not created because they already exist')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you refresh the Jupyter file browser on the left, you should now see those directories appear.  Next, let's create and display some buttons that we'll use to save snapshots\n",
    "for each class label.  We'll also add some text boxes that will display how many images of each category that we've collected so far. This is useful because we want to make\n",
    "sure we collect about as many ``free`` images as ``blocked`` images.  It also helps to know how many images we've collected overall."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "button_layout = widgets.Layout(width='128px', height='64px')\n",
    "free_button = widgets.Button(description='add free', button_style='success', layout=button_layout)\n",
    "blocked_button = widgets.Button(description='add blocked', button_style='danger', layout=button_layout)\n",
    "free_count = widgets.IntText(layout=button_layout, value=len(os.listdir(free_dir)))\n",
    "blocked_count = widgets.IntText(layout=button_layout, value=len(os.listdir(blocked_dir)))\n",
    "\n",
    "x=0 #from here TB\n",
    "controller = widgets.Controller(index=0)  # replace with index of your controller\n",
    "button_layout = widgets.Layout(width='200px', height='64px') #TB\n",
    "free_left = widgets.FloatText(layout=button_layout, value=x, description='forward') #TB\n",
    "free_right = widgets.FloatText(layout=button_layout, value=x, description='turning') #TB\n",
    "motorleft = widgets.FloatText(layout=button_layout, value=x, description='Motor Left') #TB\n",
    "motorright = widgets.FloatText(layout=button_layout, value=x, description='Motor Right') #TB\n",
    "\n",
    "speed_widget = widgets.FloatSlider(value=0.5, min=0.05, max=1.0, step=0.001, description='speed')\n",
    "#TB higher speed requires smaller turn_gain values: 2.5 for speed 0.22, around 2 for speed 0.4\n",
    "turn_gain_widget = widgets.FloatSlider(value=1, min=0.05, step=0.001, max=4.0, description='turn sensitivity')\n",
    "#TB value different for different forward speed, but very small differences\n",
    "motoradjustment_widget = widgets.FloatSlider(value=0.02, min=0.00, max=0.2, step=0.0001, description='motoradjustment')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jetbot import Robot\n",
    "import traitlets\n",
    "import math\n",
    "\n",
    "robot = Robot()\n",
    "\n",
    "#TB to show the controller values\n",
    "\n",
    "left_link = traitlets.dlink((controller.axes[1], 'value'), (free_left, 'value'), transform=lambda x: -x)\n",
    "right_link = traitlets.dlink((controller.axes[0], 'value'), (free_right, 'value'), transform=lambda x: -x)\n",
    "\n",
    "def on_value_change(change):\n",
    "    x= free_right.value\n",
    "    y= free_left.value\n",
    "    leftnew, rightnew = steering(x, y)\n",
    "    motorright.value= round(float(leftnew),2)  \n",
    "    motorleft.value= round(float(rightnew + motoradjustment_widget.value),2) # adjust the motor that lags behind\n",
    "    #motoradjustment value important to keep bot driving straight, small offset-values like 0.05\n",
    "    robot.right_motor.value=motorright.value\n",
    "    robot.left_motor.value=motorleft.value\n",
    "        \n",
    "def steering(x, y): \n",
    "    #script from stackexchange of user Pedro Werneck \n",
    "    #https://electronics.stackexchange.com/questions/19669/algorithm-for-mixing-2-axis-analog-input-to-control-a-differential-motor-drive\n",
    "    # convert to polar\n",
    "    r = math.hypot(x, y)\n",
    "    t = math.atan2(y, x)\n",
    "\n",
    "    # rotate by 45 degrees\n",
    "    t += math.pi / -4.0\n",
    "\n",
    "    # back to cartesian\n",
    "    left = r * math.cos(t)\n",
    "    right = r * math.sin(t)\n",
    "\n",
    "    # rescale the new coords\n",
    "    left = left * math.sqrt(2)\n",
    "    right = right * math.sqrt(2)\n",
    "\n",
    "    # clamp to -1/+1\n",
    "    scalefactor= speed_widget.value\n",
    "    left = max(scalefactor*-1.0, min(left, scalefactor))\n",
    "    right = max(scalefactor*-1.0, min(right, scalefactor))\n",
    "    \n",
    "    #gamma correction for response sensitivity of joystick while turning : TB\n",
    "    gamma=turn_gain_widget.value  #using slider for joystick 1-4, for object recognition 2-40 \n",
    "    if left <0 :\n",
    "        left= -1* (((abs(left)/scalefactor)**(1/gamma))*scalefactor)\n",
    "    else:\n",
    "        left= ((abs(left)/scalefactor)**(1/gamma))*scalefactor\n",
    "       \n",
    "    if right <0:\n",
    "        right= -1*(((abs(right)/scalefactor)**(1/gamma))*scalefactor)\n",
    "    else:\n",
    "        right= ((abs(right)/scalefactor)**(1/gamma))*scalefactor\n",
    "    \n",
    "    return left, right\n",
    "\n",
    "\n",
    "free_left.observe(on_value_change, names='value')\n",
    "free_right.observe(on_value_change, names='value')\n",
    "\n",
    "#left_link = traitlets.dlink((motorleft, 'value'), (robot.left_motor, 'value'))\n",
    "#right_link = traitlets.dlink((motorright, 'value'), (robot.right_motor, 'value'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jetbot import Heartbeat\n",
    "\n",
    "\n",
    "def handle_heartbeat_status(change):\n",
    "    if change['new'] == Heartbeat.Status.dead:\n",
    "        camera_link.unlink()\n",
    "        left_link.unlink()\n",
    "        right_link.unlink()\n",
    "        robot.stop()\n",
    "\n",
    "heartbeat = Heartbeat(period=0.5)\n",
    "\n",
    "# attach the callback function to heartbeat status\n",
    "heartbeat.observe(handle_heartbeat_status, names='status')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Right now, these buttons wont do anything.  We have to attach functions to save images for each category to the buttons' ``on_click`` event.  We'll save the value\n",
    "of the ``Image`` widget (rather than the camera), because it's already in compressed JPEG format!\n",
    "\n",
    "To make sure we don't repeat any file names (even across different machines!) we'll use the ``uuid`` package in python, which defines the ``uuid1`` method to generate\n",
    "a unique identifier.  This unique identifier is generated from information like the current time and the machine address."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "e93ab22b0c404f83ab2026125f30ca8a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(Image(value=b'\\xff\\xd8\\xff\\xe0\\x00\\x10JFIF\\x00\\x01\\x01\\x00\\x00\\x01\\x00\\x01\\x00\\x00\\xff\\xdb\\x00C…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "13c6288f8acf41f9bf6f1cb97183fa9c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Controller(axes=(Axis(value=0.003921627998352051), Axis(value=0.003921627998352051), Axis(value=0.003921627998…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "9ef46b998bfd4822b912a3aef618ef90",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(FloatSlider(value=0.5, description='speed', max=1.0, min=0.05, step=0.001), FloatSlider(value=1…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "642d4f01d03c4836815b8297d62521f0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(FloatText(value=-0.003921627998352051, description='forward', layout=Layout(height='64px', widt…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "01a70ead031d4f3fbc8ce14ffd7401a8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntText(value=47, layout=Layout(height='64px', width='128px')), Button(button_style='success', …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "4c70246242ef4321a6e12e1b3b8b0b52",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(IntText(value=35, layout=Layout(height='64px', width='128px')), Button(button_style='danger', d…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from uuid import uuid1\n",
    "\n",
    "snapshot_image = widgets.Image(format='jpeg', width=224, height=224)\n",
    "\n",
    "def save_snapshot(directory):\n",
    "    image_path = os.path.join(directory, str(uuid1()) + '.jpg')\n",
    "    with open(image_path, 'wb') as f:\n",
    "        f.write(image.value)\n",
    "    # display snapshot that was saved\n",
    "    snapshot_image.value = image.value\n",
    "\n",
    "def save_free(change):\n",
    "    global free_dir, free_count\n",
    "    if change['new']:\n",
    "        save_snapshot(free_dir)\n",
    "        free_count.value = len(os.listdir(free_dir))\n",
    "    \n",
    "def save_blocked(change):\n",
    "    global blocked_dir, blocked_count\n",
    "    if change['new']:\n",
    "        save_snapshot(blocked_dir)\n",
    "        blocked_count.value = len(os.listdir(blocked_dir))\n",
    "    \n",
    "def save_free_button():\n",
    "    global free_dir, free_count\n",
    "    save_snapshot(free_dir)\n",
    "    free_count.value = len(os.listdir(free_dir))\n",
    "    \n",
    "def save_blocked_button():\n",
    "    global blocked_dir, blocked_count\n",
    "    save_snapshot(blocked_dir)\n",
    "    blocked_count.value = len(os.listdir(blocked_dir))\n",
    "    \n",
    "# attach the callbacks, we use a 'lambda' function to ignore the\n",
    "# parameter that the on_click event would provide to our function\n",
    "# because we don't need it.\n",
    "\n",
    "controller.buttons[5].observe(save_free, names='value') #TB gamepad button number 5\n",
    "controller.buttons[7].observe(save_blocked, names='value') #TB gamepad button numer 7\n",
    "\n",
    "free_button.on_click(lambda x: save_free_button())\n",
    "blocked_button.on_click(lambda x: save_blocked_button())\n",
    "\n",
    "#display(image)\n",
    "display(widgets.HBox([image, snapshot_image]))\n",
    "display(controller)\n",
    "\n",
    "display(widgets.VBox([\n",
    "    speed_widget,\n",
    "    turn_gain_widget,\n",
    "    motoradjustment_widget,\n",
    "]))\n",
    "\n",
    "display(widgets.HBox([free_left, free_right, motorleft, motorright]))\n",
    "\n",
    "display(widgets.HBox([free_count, free_button]))\n",
    "display(widgets.HBox([blocked_count, blocked_button]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "camera.unobserve_all()\n",
    "time.sleep(1.0)\n",
    "robot.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! Now the buttons above should save images to the ``free`` and ``blocked`` directories.  You can use the Jupyter Lab file browser to view these files!\n",
    "\n",
    "Now go ahead and collect some data \n",
    "\n",
    "1. Place the robot in a scenario where it's blocked and press ``add blocked``\n",
    "2. Place the robot in a scenario where it's free and press ``add free``\n",
    "3. Repeat 1, 2\n",
    "\n",
    "> REMINDER: You can move the widgets to new windows by right clicking the cell and clicking ``Create New View for Output``.  Or, you can just re-display them\n",
    "> together as we will below\n",
    "\n",
    "Here are some tips for labeling data\n",
    "\n",
    "1. Try different orientations\n",
    "2. Try different lighting\n",
    "3. Try varied object / collision types; walls, ledges, objects\n",
    "4. Try different textured floors / objects;  patterned, smooth, glass, etc.\n",
    "\n",
    "Ultimately, the more data we have of scenarios the robot will encounter in the real world, the better our collision avoidance behavior will be.  It's important\n",
    "to get *varied* data (as described by the above tips) and not just a lot of data, but you'll probably need at least 100 images of each class (that's not a science, just a helpful tip here).  But don't worry, it goes pretty fast once you get going :)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Next\n",
    "\n",
    "Once you've collected enough data, we'll need to copy that data to our GPU desktop or cloud machine for training.  First, we can call the following *terminal* command to compress\n",
    "our dataset folder into a single *zip* file.\n",
    "\n",
    "> The ! prefix indicates that we want to run the cell as a *shell* (or *terminal*) command.\n",
    "\n",
    "> The -r flag in the zip command below indicates *recursive* so that we include all nested files, the -q flag indicates *quiet* so that the zip command doesn't print any output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "!zip -r -q dataset.zip dataset"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see a file named ``dataset.zip`` in the Jupyter Lab file browser.  You should download the zip file using the Jupyter Lab file browser by right clicking and selecting ``Download``.\n",
    "\n",
    "Next, we'll need to upload this data to our GPU desktop or cloud machine (we refer to this as the *host*) to train the collision avoidance neural network.  We'll assume that you've set up your training\n",
    "machine as described in the JetBot WiKi.  If you have, you can navigate to ``http://<host_ip_address>:8888`` to open up the Jupyter Lab environment running on the host.  The notebook you'll need to open there is called ``collision_avoidance/train_model.ipynb``.\n",
    "\n",
    "So head on over to your training machine and follow the instructions there!  Once your model is trained, we'll return to the robot Jupyter Lab enivornment to use the model for a live demo!"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
